#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S1,     ,                    sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motorextensionleft, tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorLEFT,     tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorextensionright, tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorRIGHT,    tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_2,    servo6,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    gateright,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    gateleft,             tServoStandard)
#pragma config(Sensor, S2,     HTGYRO,              sensorAnalogInactive)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//#pragma config(Servo,  srvo_S1_C2_1,    servo1,               tServoStandard)
//#pragma config(Servo,  srvo_S1_C2_2,    servo2,               tServoStandard)
//#pragma config(Servo,  srvo_S1_C2_3,    SwingL,               tServoStandard)
//#pragma config(Servo,  srvo_S1_C2_4,    SwingR,               tServoStandard)
//#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
//#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)


//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"
#include "AutonomousCommon.c"

//TJoystick joystick;

void gyrotest ()
{
  int tophat = joystick.joy1_TopHat;
  switch (tophat)
  {

  case 0:
    Face (0);
    break;

  case 1:
    Face (45);
    break;

  case 2:
    Face (90);
    break;

  case 3:
    Face (135);
    break;

  case 4:
    Face (180);
    break;

  case 5:
    Face (225);
    break;

  case 6:
    Face (270);
    break;

  case 7:
    Face (315);
    break;

  default:
    wait10Msec (5);
    break;
  }
  if (nNxtButtonPressed == kEnterButton)
  {
    resetGyro = true;
  }
  eraseDisplay();

  nxtDisplayTextLine(1, "Reading");
  // Read the current calibration offset and display it
  nxtDisplayTextLine(2, "Offset: %4d ", gyroCal);
  nxtDisplayTextLine (3, "drift: %4.2f", driftrate);

  nxtDisplayClearTextLine(4);
  nxtDisplayTextLine(4, "Gyro:   %4d", gyro);
  nxtDisplayTextLine(5, "curAngle:   %4.1f", curAngle);
  nxtDisplayTextLine(6, "Press enter");
  nxtDisplayTextLine(7, "to recalibrate");
}

void motorControlProportional()
{
  float nMotorPower;
  float nDirection;
  float percent_left;
  float percent_right;

  nMotorPower = joystick.joy1_y1;
  nDirection  = joystick.joy1_x2;

  percent_left = (nDirection + nMotorPower ) / 128 ;
  percent_right =  (-nDirection + nMotorPower ) / 128 ;


  if     (joystick.joy1_Buttons != 0x80)
  {
    motor[motorLEFT] = -percent_left * 20 ;
    motor[motorRIGHT] = percent_right * 20 ;

  }
  else
  {
    motor[motorLEFT] = -percent_left * 128;
    motor[motorRIGHT] = percent_right * 128 ;
  }
}
int DOWN = 1;
int UP = 37;
int SWINGposition = DOWN;

void gatecontrol ()

{

  if     (joystick.joy2_Buttons == 0x08)
  {
    SWINGposition = UP;
  }

  if     (joystick.joy2_Buttons == 0x02)
  {
    SWINGposition = DOWN;
  }

  if      (SWINGposition == UP)
  {
    gateup();
  }

  else
  {
    gatedown();
  }

  //servo [SwingL] = SWINGposition ;
  //servo [SwingR] = -SWINGposition ;
}

task main()
{ InitAuton ();
  while (true)
  {
    getJoystickSettings(joystick);
    motorControlProportional ();
    gatecontrol ();\
    // motor[motorD] = joystick.joy1_y1;
    // motor[motorE] = -joystick.joy1_y2;
  }
}
